%--------------------------------------------------------------------------
%   粒子群优化算法
%   Particle Swarm Optimization
%--------------------------------------------------------------------------
%   输入
%   func        函数句柄(输入变量按照向量排列[x y z ...])
%   Particle    粒子数量
%   limit       粒子定义域，按照维数设定[minX maxX;minY maxY;minZ maxZ;...]
%   vlim        速度定义域[minV maxV]
%   err         收敛条件,到达一定误差跳出循环
%   example:
% f = @(x) 3*(1-x(1)).^2.*exp(-(x(1).^2) - (x(2)+1).^2) ... 
%    - 10*(x(1)/5 - x(1).^3 - x(2).^5).*exp(-x(1).^2-x(2).^2) ... 
%    - 1/3*exp(-(x(1)+1).^2 - x(2).^2);
%   PSO(f,50,[-3 3;-3 3],[-1 1],1e-5)
%--------------------------------------------------------------------------
function gbest = PSO(func,Particle,limit,vlim,err)

N = Particle;                                                               %粒子个数
iter = 300;                                                                 %迭代次数
w = 1;                                                                      %惯性权重
c1 = 0.8;                                                                   %自我学习权重
c2 = 0.3;                                                                   %群体学习权重
d = size(limit,1);
[temp,fpbest,nfpbest,fgworst] = deal(zeros(1,N));                           %初始化变量
%--------------------------------------------------------------------------
%   算法部分
%--------------------------------------------------------------------------
%   初始化种群位置
%--------------------------------------------------------------------------
pos = zeros(d,N);
for idx = 1:d
    pos(idx,:) = rand(1,N).*(limit(idx,2)-limit(idx,1))+limit(idx,1);
end

%--------------------------------------------------------------------------
%   初始化种群速度
%--------------------------------------------------------------------------
v = zeros(d,N);
for idx = 1:d
    v(idx,:) = rand(1,N).*(vlim(2)-vlim(1))+vlim(1);
end

%--------------------------------------------------------------------------
%   首次运行初始化缓冲区
%--------------------------------------------------------------------------
pbest = pos;                                                                %粒子最佳位置

for idx = 1:N
    fpbest(:,idx) = func(pbest(:,idx));                                     %当前粒子输出
end
[~,gbest_loc] = min(fpbest);
gbest =  pos(:,gbest_loc);                                                  %种群最佳位置

fgbest = func(gbest);                                                       %种群最佳输出

for idx = 1:iter
    if idx==50                                                              %迭代收敛
        w = 0.5;
        c1 = 0.3;
        c2 = 0.8; 
    end
    if idx==100                                                             %迭代收敛
        w = 0.5;
        c1 = 0.2;
        c2 = 0.8; 
    end
    %----------------------------------------------------------------------
    %   更新速度+边界约束
    %----------------------------------------------------------------------
    v = v.* w + c1 .* rand(d,N) .*(pbest - pos) + c2.*rand(d,1).*(gbest - pos);
    v(v > vlim(2)) = vlim(2);
    v(v < vlim(1)) = vlim(1);
    %----------------------------------------------------------------------
    %   更新位置+边界约束
    %----------------------------------------------------------------------
    pos = pos + v;
    for jdx = 1:d
        temp = pos(jdx,:);
        temp(temp>limit(jdx,2))=limit(jdx,2);
        temp(temp<limit(jdx,1))=limit(jdx,1);
        pos(jdx,:) = temp;
    end
    
    %----------------------------------------------------------------------
    %   更新粒子最优
    %----------------------------------------------------------------------
    for jdx = 1:N
        nfpbest(:,jdx) = func(pos(:,jdx));
    end
    L = nfpbest<fpbest;
    pbest(:,L) = pos(:,L);
    for jdx = 1:N
        fpbest(:,jdx) = func(pbest(:,jdx));
    end
    %----------------------------------------------------------------------
    %   更新粒子最差
    %----------------------------------------------------------------------
    for jdx = 1:N
        fgworst(:,jdx) = func(pos(:,jdx));
    end
    fgworst = max(fgworst);
    
    %----------------------------------------------------------------------
    %   更新全局最优
    %----------------------------------------------------------------------
    for jdx = 1:N
        temp(:,jdx) = func(pbest(:,jdx));
    end
    [~,gbest_loc2] = min(temp);
    ngbest = pos(:,gbest_loc2);
    nfgbest = func(ngbest);
    
    %----------------------------------------------------------------------
    %   最好最差循环跳出
    %----------------------------------------------------------------------
    if abs(fgworst-fgbest)<err
        disp(['符合条件:最佳与最差粒子结果 < ' num2str(err) ' 退出'])
        break
    end
    
    
    if nfgbest<fgbest
        gbest = ngbest;
        fgbest = nfgbest;
    end

end
end
